#include "quaternionf.h"


// 
//	Constructor
//

// From raw quaternion values
quaternionf::quaternionf(float x, float y, float z, float w) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
};
// From Euler angles
quaternionf::quaternionf(float pitch, float yaw, float roll) {
	// TODO
};
// From Axis/Angle
quaternionf::quaternionf(const vec3f &axis, float angle) {
	// TODO
};
// From rotation matrix
quaternionf::quaternionf(const mat4f &m) {
	// TODO
};


/* * * * * * * * * * Static Quaternion functions * * * * * * * * * */

//
//	Euler angles to quaternion
//
quaternionf	quaternionf::eulerAnglesToQuaternion(float pitch, float yaw, float roll) {
	float y2 = (float)DegToRad(yaw/2.0f);
	float p2 = (float)DegToRad(pitch/2.0f);
	float r2 = (float)DegToRad(roll/2.0f);

	float cosy   = (float)cos(y2);
	float cosp   = (float)cos(p2);
	float cosr   = (float)cos(r2);

	float siny   = (float)sin(y2);
	float sinp   = (float)sin(p2);
	float sinr   = (float)sin(r2);

	quaternionf q;

	q.x = cosr * sinp * cosy + sinr * cosp * siny;
	q.y = cosr * cosp * siny - sinr * sinp * cosy;
	q.z = sinr * cosp * cosy - cosr * sinp * siny;
	q.w = cosr * cosp * cosy + sinr * sinp * siny;

	return q;
};

//
//	Axis angle to quaternion
//
quaternionf quaternionf::axisAngleToQuaternion(const vec3f &vector, float degrees) {
	float theta = (float)DegToRad(degrees);
	float result = (float)sin(theta / 2.0f);
	return quaternionf((float)(vector.x * result), (float)(vector.y * result), (float)(vector.z * result), (float)cos(theta / 2.0f));
};

//
//	Rotate point by quaternion
//
/*
void quaternionf::rotatePointByQuaternion(const quaternionf &q, vec3f &point) {
};
*/

//
//	From Matrix
//
quaternionf quaternionf::fromMatrix(const mat4f &m) {
	quaternionf q;

	q.w = sqrt( max( 0.0f, (1.0f + m.i[0] + m.i[5] + m.i[10]) ) ) / 2;
	q.x = sqrt( max( 0.0f, (1.0f + m.i[0] - m.i[5] - m.i[10]) ) ) / 2;
	q.y = sqrt( max( 0.0f, (1.0f - m.i[0] + m.i[5] - m.i[10]) ) ) / 2;
	q.z = sqrt( max( 0.0f, (1.0f - m.i[0] - m.i[5] + m.i[10]) ) ) / 2;

	q.x = (float)_copysign( q.x, m.i[9] - m.i[6] );
	q.y = (float)_copysign( q.y, m.i[2] - m.i[8] );
	q.z = (float)_copysign( q.z, m.i[4] - m.i[1] );

	return q;
};

//
//	Dot product
//
float quaternionf::dot(const quaternionf &a, const quaternionf &b) {
	return (a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w);
};


/* * * * * * * * * * Quaternion functions * * * * * * * * * */

//
//	Normalize
//
void quaternionf::normalize() {
	float magnitude	= sqrt(dot(*this, *this));
	if(magnitude > 0.0f) {
		float t = 1.0f / magnitude;
		x *= t;
		y *= t;
		z *= t;
		w *= t;
	}
};


//
//	ToMatrix
//
mat4f quaternionf::toMatrix() const {
	mat4f m;
	float yy = y * y;
	float zz = z * z;
	float xy = x * y;
	float zw = z * w;
	float xz = x * z;
	float yw = y * w;
	float xx = x * x;
	float yz = y * z;
	float xw = x * w;

	m.i[0] =	1 -		2 *		yy -	2 *		zz;
	m.i[1] =	2 *		xy +	2 *		zw;
	m.i[2] =	2 *		xz -	2 *		yw;	

	m.i[4] =	2 *		xy -	2 *		zw;
	m.i[5] =	1 -		2 *		xx -	2 *		zz;
	m.i[6] =	2 *		yz +	2 *		xw;

	m.i[8] =	2 *		xz +	2 *		yw;
	m.i[9] =	2 *		yz -	2 *		xw;
	m.i[10] =	1 -		2 *		xx -	2 *		yy;

	return m;
};


//
//	Conjugate (quaternion inverse)
//
quaternionf quaternionf::conjugate() const {
	return quaternionf(-x, -y, -z, w);
};


//
//	Generate w value
//
void quaternionf::generatew() {
	w = 1.0f - (x * x) - (y * y) - (z * z);
	if(w < 0.0f) {
		w = 0.0f;
	} else {
		w = -sqrt(w);
	}
};


/* * * * * * * * * * Quaternion Operators * * * * * * * * * */

// 
//	quaternion * quaternion
//
quaternionf quaternionf::operator*(const quaternionf &b) const {
	quaternionf ans;

	ans.w = (w * b.w) - (x * b.x) - (y * b.y) - (z * b.z);
	ans.x = (x * b.w) + (w * b.x) + (y * b.z) - (z * b.y);
	ans.y = (y * b.w) + (w * b.y) + (z * b.x) - (x * b.z);
	ans.z = (z * b.w) + (w * b.z) + (x * b.y) - (y * b.x);

	return ans;
};

//
//	quaternion * vec3f
//
quaternionf quaternionf::operator*(const vec3f &b) const {
	quaternionf ans;

	ans.w = -(x * b.x) - (y * b.y) - (z * b.z);
	ans.x =  (w * b.x) + (y * b.z) - (z * b.y);
	ans.y =  (w * b.y) + (z * b.x) - (x * b.z);
	ans.z =  (w * b.z) + (x * b.y) - (y * b.x);

	return ans;
};